{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/Users/malavikabindhi/anaconda3/lib/python3.6/site-packages/h5py/__init__.py:34: FutureWarning: Conversion of the second argument of issubdtype from `float` to `np.floating` is deprecated. In future, it will be treated as `np.float64 == np.dtype(float).type`.\n",
      "  from ._conv import register_converters as _register_converters\n",
      "Using TensorFlow backend.\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import sys\n",
    "import os\n",
    "import tensorflow as tf\n",
    "from keras import optimizers\n",
    "from keras.layers import Input, Add, concatenate\n",
    "from keras.layers.merge import Concatenate\n",
    "from keras.models import Model\n",
    "from keras.layers import Dense, Flatten, Reshape, Dropout\n",
    "from keras.layers import Convolution1D, MaxPooling1D, BatchNormalization\n",
    "from keras.layers import Lambda\n",
    "from keras.utils import np_utils\n",
    "from keras.models import load_model\n",
    "import h5py\n",
    "from matplotlib.pyplot import imshow\n",
    "import cv2\n",
    "import glob\n",
    "import math\n",
    "from keras.preprocessing.image import load_img\n",
    "from keras.preprocessing.image import img_to_array\n",
    "from keras.applications.imagenet_utils import decode_predictions\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "PATH TO DATA:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#testing the data processing\n",
    "pcd_path = \"/Volumes/Hard Drive/data_object_velodyne/training/velodyne/\"\n",
    "label_path = \"/Volumes/Hard Drive/training/label_2/\"\n",
    "calib_path = \"/Volumes/Hard Drive/data_object_calib/training/calib/\"\n",
    "img_path = \"/Volumes/Hard Drive/data_object_image_2/training/image_2/\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Generate Training Data for PointNet:\n",
    "(Data generated, processed, and stored in 'train_points.npy' and 'train_labels.npy')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#loading files and paths\n",
    "BASE_DIR = os.path.dirname(os.path.abspath('__file__'))\n",
    "sys.path.append(BASE_DIR)\n",
    "\n",
    "def load_pc_from_bin(bin_path):\n",
    "    \"\"\"Load a bin file to a np array\"\"\"\n",
    "    num_select = 2048\n",
    "    selected_points =[]\n",
    "    obj = np.fromfile(bin_path, dtype = np.float32).reshape(-1,4)\n",
    "    pc = filter_camera_angle(obj)\n",
    "    index = np.random.choice(pc.shape[0], num_select, replace=False)\n",
    "    for i in range(len(index)):\n",
    "        selected_points.append(pc[index[i]][0:3])\n",
    "    selected_points = np.array(selected_points).reshape(1,-1,3)# return N*3 array\n",
    "    return selected_points\n",
    "\n",
    "\n",
    "\n",
    "def read_calib_file(calib_path):\n",
    "    \"\"\"Read a calibration file.\"\"\"\n",
    "    data = {}\n",
    "    with open(calib_path, 'r') as f:\n",
    "        for line in f.readlines():\n",
    "            if not line or line == \"\\n\":\n",
    "                continue\n",
    "            key, value = line.split(':', 1)\n",
    "            try:\n",
    "                data[key] = np.array([float(x) for x in value.split()])\n",
    "            except ValueError:\n",
    "                pass\n",
    "    return data\n",
    "\n",
    "\n",
    "def proj_to_velo(calib_data):\n",
    "    \"\"\"Projection matrix to 3D axis for 3D Label\"\"\"\n",
    "    rect = calib_data[\"R0_rect\"].reshape(3, 3)\n",
    "    velo_to_cam = calib_data[\"Tr_velo_to_cam\"].reshape(3, 4)\n",
    "    inv_rect = np.linalg.inv(rect)\n",
    "    inv_velo_to_cam = np.linalg.pinv(velo_to_cam[:, :3])\n",
    "    return np.dot(inv_velo_to_cam, inv_rect)\n",
    "\n",
    "\n",
    "def read_label_from_txt(label_path):\n",
    "    \"\"\"Read label from txt file.\"\"\"\n",
    "    text = np.fromfile(label_path)\n",
    "    bounding_box = []\n",
    "    with open(label_path, \"r\") as f:\n",
    "        labels = f.read().split(\"\\n\")\n",
    "        for label in labels:\n",
    "            if not label:\n",
    "                continue\n",
    "            label = label.split(\" \")\n",
    "            if (label[0] == \"DontCare\"):\n",
    "                continue\n",
    "            y_class = [int(label[0]==\"Car\"), int(label[0]==\"Van\"), int(label[0]==\"Pedestrian\")]\n",
    "            if label[0] == (\"Car\") or label[0]==\"Van\" or label[0]==\"Pedestrian\": #  or \"Truck\"\n",
    "                y_labels = list(np.hstack((label[8:15], y_class)))\n",
    "                bounding_box.append(y_labels)\n",
    "                break\n",
    "\n",
    "    if bounding_box:\n",
    "        data = np.array(bounding_box, dtype=np.float32)\n",
    "        return data[:, 3:6], data[:, :3], data[:, 6], data[:,7:] \n",
    "    else:\n",
    "        return None, None, None, None\n",
    "\n",
    "def read_labels(label_path, label_type, calib_path=None, is_velo_cam=False, proj_velo=None):\n",
    "    \"\"\"Read labels from txt file.\n",
    "    Original Label value is shifted about 0.27m from object center.\n",
    "    So need to revise the position of objects.\n",
    "    \"\"\"\n",
    "    if label_type == \"txt\": #TODO\n",
    "        places, size, rotates, y_class = read_label_from_txt(label_path)\n",
    "        if places is None:\n",
    "            return None, None, None, None\n",
    "        rotates = np.pi / 2 - rotates\n",
    "        dummy = np.zeros_like(places)\n",
    "        dummy = places.copy()\n",
    "        if calib_path:\n",
    "            places = np.dot(dummy, proj_velo.transpose())[:, :3]\n",
    "        else:\n",
    "            places = dummy\n",
    "        if is_velo_cam:\n",
    "            places[:, 0] += 0.27\n",
    "\n",
    "    data_combined = []\n",
    "    for p, r, s , cl in zip(places, rotates, size, y_class):\n",
    "        ps = np.hstack((cl[:],p[:],s[:]))\n",
    "        data_combined.append(list(np.append(ps, r)))\n",
    "        \n",
    "    return places, rotates, size, y_class\n",
    "\n",
    "def get_boxcorners(places, rotates, size):\n",
    "    \"\"\"Create 8 corners of bounding box from bottom center.\"\"\"\n",
    "    corners = []\n",
    "    for place, rotate, sz in zip(places, rotates, size):\n",
    "        x, y, z = place\n",
    "        h, w, l = sz\n",
    "        if l > 10:\n",
    "            continue\n",
    "\n",
    "        corner = np.array([\n",
    "            [x - l / 2., y - w / 2., z],\n",
    "            [x + l / 2., y - w / 2., z],\n",
    "            [x - l / 2., y + w / 2., z],\n",
    "            [x - l / 2., y - w / 2., z + h],\n",
    "            [x - l / 2., y + w / 2., z + h],\n",
    "            [x + l / 2., y + w / 2., z],\n",
    "            [x + l / 2., y - w / 2., z + h],\n",
    "            [x + l / 2., y + w / 2., z + h],\n",
    "        ])\n",
    "\n",
    "        corner -= np.array([x, y, z])\n",
    "\n",
    "        rotate_matrix = np.array([\n",
    "            [np.cos(rotate), -np.sin(rotate), 0],\n",
    "            [np.sin(rotate), np.cos(rotate), 0],\n",
    "            [0, 0, 1]\n",
    "        ])\n",
    "\n",
    "        a = np.dot(corner, rotate_matrix.transpose())\n",
    "        a += np.array([x, y, z])\n",
    "        corners.append(a)\n",
    "    return np.array(corners)\n",
    "\n",
    "def filter_camera_angle(places):\n",
    "    \"\"\"Filter camera angles for KiTTI Datasets\"\"\"\n",
    "    bool_in = np.logical_and((places[:, 1] < places[:, 0] - 0.27), (-places[:, 1] < places[:, 0] - 0.27))\n",
    "    # bool_in = np.logical_and((places[:, 1] < places[:, 0]), (-places[:, 1] < places[:, 0]))\n",
    "    return places[bool_in]\n",
    "\n",
    "def center_to_sphere(places, size, resolution=0.50, min_value=np.array([0., -50., -4.5]), scale=4, x=(0, 90), y=(-50, 50), z=(-4.5, 5.5)):\n",
    "    \"\"\"Convert object label to Training label for objectness loss\"\"\"\n",
    "    x_logical = np.logical_and((places[:, 0] < x[1]), (places[:, 0] >= x[0]))\n",
    "    y_logical = np.logical_and((places[:, 1] < y[1]), (places[:, 1] >= y[0]))\n",
    "    z_logical = np.logical_and((places[:, 2] < z[1]), (places[:, 2] >= z[0]))\n",
    "    xyz_logical = np.logical_and(x_logical, np.logical_and(y_logical, z_logical))\n",
    "    center = places.copy()\n",
    "    center[:, 2] = center[:, 2] + size[:, 0] / 2.\n",
    "    sphere_center = ((center[xyz_logical] - min_value) / (resolution * scale)).astype(np.int32)\n",
    "    return sphere_center\n",
    "\n",
    "def sphere_to_center(p_sphere, resolution=0.5, scale=4, min_value=np.array([0., -50., -4.5])):\n",
    "    \"\"\"from sphere center to label center\"\"\"\n",
    "    center = p_sphere * (resolution*scale) + min_value\n",
    "    return center\n",
    "\n",
    "#derived from yukitsuji/3D_CNN_tensorflow\n",
    "def process(velodyne_path, label_path=None, calib_path=None, dataformat=\"bin\", label_type=\"txt\", is_velo_cam=False):\n",
    "    p = []\n",
    "    pc = None\n",
    "    bounding_boxes = None\n",
    "    places = None\n",
    "    rotates = None\n",
    "    size = None\n",
    "    proj_velo = None\n",
    "    \n",
    "    filenames_velo = [d for d in sorted(os.listdir(velodyne_path)) if d[0]!='.']\n",
    "    train_points = None\n",
    "    train_labels = None\n",
    "    train_classes = None\n",
    "    for d in filenames_velo:\n",
    "        value = d[0:6]\n",
    "        print(value)\n",
    "        velo_path = velodyne_path + value + '.bin'\n",
    "        cal_path = calib_path + value + '.txt'\n",
    "        lab_path = label_path + value + '.txt'\n",
    "        cur_points = load_pc_from_bin(velo_path)\n",
    "        cur_calib = read_calib_file(cal_path)\n",
    "        proj_velo = proj_to_velo(cur_calib)[:, :3]\n",
    "        places, rotates, size, classes= read_labels(lab_path, label_type, cal_path , is_velo_cam=is_velo_cam, proj_velo=proj_velo)\n",
    "        corners = get_boxcorners(places, rotates, size)\n",
    "        if places is None:\n",
    "            continue\n",
    "        if train_points is None:\n",
    "            train_labels = corners\n",
    "            train_points = cur_points\n",
    "            train_classes = classes\n",
    "        else:\n",
    "            train_points = np.concatenate((train_points, cur_points), axis = 0)\n",
    "            train_labels = np.concatenate((train_labels, corners), axis = 0)\n",
    "            train_classes = np.concatenate((train_classes, classes), axis =0)\n",
    "            \n",
    "    return train_points, train_labels, train_classes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "train_points, train_labels, train_classes = process(pcd_path, label_path, calib_path=calib_path, dataformat=\"bin\", is_velo_cam=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 91,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(7481, 3)\n"
     ]
    }
   ],
   "source": [
    "print(train_classes.shape)\n",
    "np.save('train_classes.npy', train_classes)\n",
    "np.save('train_points.npy', train_points)\n",
    "np.save('train_labels.npy', train_labels)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 70,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(7481, 24)\n"
     ]
    }
   ],
   "source": [
    "print(train_labels.reshape((7481,24)).shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Generate image data and processing using Resnet-50 model:\n",
    "(Data generated, processed, and activation layer is stored in 'intermediate_output.npy')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#run this cell only if images need to be processed again\n",
    "filenames_imgs = [img_path+d for d in sorted(os.listdir(img_path)) if d[0]!='.']\n",
    "batch_images = None\n",
    "for d in filenames_imgs:\n",
    "    print(d)\n",
    "    original = load_img(d, target_size=(224, 224))\n",
    "    numpy_image = img_to_array(original)\n",
    "    image_batch_cur = np.expand_dims(numpy_image, axis=0)\n",
    "    if batch_images is None:\n",
    "        batch_images = image_batch_cur\n",
    "    else:\n",
    "        batch_images = np.concatenate((batch_images,image_batch_cur ), axis = 0)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from keras.applications import resnet50\n",
    "resnet_model = resnet50.ResNet50(weights='imagenet')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#run this cell only if images need to be processed again. Otherwise, use the saved file in next cell\n",
    "processed_image = resnet50.preprocess_input(batch_images.copy())\n",
    "np.save('processed_image.npy', processed_image)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#run this cell only if images need to be processed again by resnet. Otherwise, use the saved file in next cell\n",
    "processed_images = np.load('processed_images.npy')\n",
    "\n",
    "init = tf.global_variables_initializer()\n",
    "sess = tf.Session()\n",
    "sess.run(init)\n",
    "\n",
    "#extract final layer called flatten_1 with size 1 by 2048\n",
    "layer_name = 'flatten_1'\n",
    "layer = resnet_model.get_layer(layer_name)\n",
    "\n",
    "#layer = resnet_model.layers[layer_num]\n",
    "intermediate_layer_model = Model(inputs=resnet_model.input,\n",
    "                                 outputs=layer.output)\n",
    "intermediate_output = intermediate_layer_model.predict(processed_images)\n",
    "print(\"Shape of Processed Images: \", processed_images.shape)\n",
    "print(\"Layer used: \", layer.name)\n",
    "print(\"Shape of layer output:\", intermediate_output.shape)\n",
    "\n",
    "np.save('intermediate_output.npy', intermediate_output)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(7481, 1, 2048)\n"
     ]
    }
   ],
   "source": [
    "intermediate_output = np.load('intermediate_output.npy')\n",
    "print(intermediate_output.shape)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
